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Abstract 

Autonomous  operations  are  vital  to  future  naval  operations.  Unmanned  systems, 
including  autonomous  underwater  vehicles  (AUVs)  and  autonomous  surface  vehicles 
(AS Vs),  are  anticipated  to  play  a  key  role  for  critical  tasks  such  as  mine  counter¬ 
measures  (MCM)  and  anti-submarine  warfare  (ASW).  Addressing  these  issues  with 
autonomous  systems  poses  a  host  of  difficult  research  challenges,  including  sensing, 
power,  acoustic  communications,  navigation,  and  autonomous  decision-making. 

This  thesis  addresses  the  issues  of  sensing  and  autonomy,  studying  the  benefits 
of  adaptive  motion  in  overcoming  partial  observability  of  sensor  observations.  We 
focus  on  the  challenge  of  target  tracking  with  range-only  measurements,  relying  on 
adaptive  motion  to  localize  and  track  maneuvering  targets.  Our  primary  contribution 
has  been  to  develop  new  MOOS-IvP  autonomy  and  state  estimation  modules  to  enable 
an  autonomous  surface  vehicle  to  locate  and  track  a  submerged  contact  using  range- 
only  sensor  information.  These  capabilities  were  initially  tested  in  simulation  for 
increasing  levels  of  complexity  of  target  motion,  and  subsequently  evaluated  in  a  field 
test  with  a  Kingfisher  ASV.  Our  results  demonstrate  the  feasibility,  in  a  controlled 
environment,  to  localize  and  track  a  maneuvering  undersea  target  using  range-only 
measurements. 
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Chapter  1 


Introduction 


1.1  Thesis  Goal 


Autonomous  operations  are  vital  to  future  United  States  naval  operations.  Unmanned 
systems,  including  autonomous  underwater  vehicles  (AUVs)  and  autonomous  surface 
vehicles  (AS Vs),  are  envisioned  as  playing  a  key  role  in  critical  tasks  such  as  mine 
counter-measures  (MCM)  and  anti-submarine  warfare  (ASW).  Addressing  these  is¬ 
sues  with  autonomous  systems  poses  a  host  of  difficult  research  challenges,  includ¬ 
ing  sensing,  power,  acoustic  communications  [2,20],  navigation  [18,21,22],  and  au¬ 
tonomous  decision-making  [10]. 

In  this  thesis,  we  address  the  issues  of  sensing  and  autonomy,  studying  the  ben¬ 
efits  of  adaptive  motion  in  overcoming  partial  observability  of  sensor  observations. 
We  focus  on  the  challenge  of  target  tracking  with  partial  (range-only)  information, 
relying  on  adaptive  motion  to  localize  and  track  maneuvering  targets.  Our  primary 
contribution  has  been  to  develop  new  MOOS-IvP  [11]  autonomy  and  state  estimation 
modules  to  enable  an  autonomous  surface  vehicle  to  locate  and  track  a  submerged 
contact  using  range-only  sensor  information.  These  capabilities  were  initially  tested 
in  simulation  for  increasing  levels  of  complexity  of  target  motion,  and  subsequently 
evaluated  in  a  field  test  with  a  Kingfisher  ASV. 
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1.2  Problem  Statement 


To  confine  the  bounds  of  the  problem,  we  made  the  following  assumptions.  This  thesis 
focuses  on  tracking  a  single  vehicle  and  uses  a  simple  sonar  range  measurement  model 
for  the  range  information  produced  by  a  simulated  active  sonar  application  with  at 
least  five  range  measurements  prior  to  the  contact  maneuvering.  A  single  vehicle 
was  chosen  do  minimize  the  complexity  caused  by  both,  disambiguating  the  contact’s 
location  and  correlating  the  range  data  to  the  proper  contact.  The  simulated  sonar 
information  received  was  in  the  form  of  direct  path  propagation;  bottom  bounce  and 
convergence  zone  signal  sources  were  not  modeled.  The  size  of  the  submerged  contact 
was  on  the  order  of  10  m  or  less.  Lastly,  the  setting  for  this  thesis  was  in  a  port  or 
harbor  area  (area  of  1  km2  or  less),  not  in  an  open  ocean  environment. 

1.3  Background 

1.3.1  Submerged  Contact  Tracking 

The  goal  for  submerged  contact  tracking  is  to  be  able  to  track  a  submerged  mobile 
contact  given  uncertain  acoustic  measurements  of  the  range  and/or  bearing  to  the 
target.  There  are  two  basic  ways  for  gathering  information  on  a  submerged  source: 
passive  sonar  or  active  sonar.  Passive  sonar  relies  on  the  ability  to  listen  to  underwater 
sounds.  An  individual  hydrophone  or  an  array  of  hydrophones  can  perform  passive 
sonar  by  receiving  and  processing  sound  waves.  Examples  of  these  are  towed  arrays 
and  permanent  fixtures  like  the  sound  surveillance  system  (SOSUS)  array.  Active 
sonar  involves  emitting  a  pulse  of  energy  from  a  transmitter  and  sensing  the  reflected 
wave  in  the  receiver.  The  range  is  found  by  measuring  the  time  difference,  At  of  the 
emitted  and  reflected  waves.  Because  the  speed  of  sound,  vsy  in  the  environment  is 
known,  the  range  can  be  calculated  by  using  the  following  equation:  range,  r,  equals 
speed  of  sound  multiplied  by  time,  r  =  vs  *  At. 

Generally,  passive  sonar  is  used  in  situations  in  which  the  detector  does  not  want 
to  be  detected  and  active  sonar  is  used  when  that  is  not  a  concern.  Active  sonar 
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can  also  control  the  energy  level  of  the  emitted  pulse  to  overcome  interfering  noise 
from  the  moving  detector  and  control  the  frequency  at  which  the  pulse  is  transmitted. 
Because  of  this,  and  the  lack  of  need  for  stealth,  simulated  active  sonar  was  used  in 
this  research. 

Once  the  range  data  has  been  received  and  processed,  target  motion  analysis 
(TMA)  techniques  are  applied  in  concert  with  an  estimation  filter  to  conduct  lo¬ 
calization  and  perform  tracking.  TMA  techniques  are  also  used  to  overcome  the 
problem  of  ambiguity  that  results  from  non-observability.  Non-observability  results 
from  having  a  set  of  ranges  that  can  produce  an  infinite  number  of  course,  speed,  and 
coordinate  possibilities.  This  topic  is  described  in  more  detail  in  Chapter  4. 

At  the  core  of  the  tracking  problem  is  the  use  of  a  recursive  state  estimator.  Many 
estimators  were  considered,  including  Extended  Kalman  Filtering  (EKF),  Maximum 
Likelihood  Estimation  (MLE),  and  Particle  Filtering  (PF)  [12].  For  this  research,  a 
particle  filter  was  chosen  for  the  following  reasons:  (1)  the  ability  to  handle  non-linear 
systems  of  equations,  such  as  solving  for  coordinates  and  course  based  on  range;  (2) 
the  ability  to  operate  without  distribution  assumptions;  (3)  the  capability  to  adapt 
to  moving  targets;  (4)  the  production  of  multiple  hypothesis  metrics  that  could  be 
directly  used  in  MOOS-IvP,  (5)  its  relative  performance  in  regard  to  Kalman  filtering 
and  maximum  likelihood  estimation  [31]. 

1.3.2  Autonomous  Surface  Vehicles 

One  of  the  secondary  goals  of  this  project  is  to  develop  an  inexpensive,  reliable,  and 
feasible  way  to  track  and  localize  underwater  contacts  using  current  ASVs  available  at 
the  Massachusetts  Institute  of  Technology  (MIT).  MIT  owns  and  operates  two  types 
of  ASVs:  the  SCOUT  platform,  a  robotic  kayak  that  costs  roughly  $30,000  per  unit, 
and  a  Clearpath  Robotic’s  Kingfisher  (Figure  1-1),  that  costs  roughly  $20,000  per 
unit.  Both  of  these  ASVs  are  equipped  with  GPS,  compasses,  WiFi  modems,  and  a 
PC  that  has  been  configured  with  MOOS-IvP  autonomy  software.  To  keep  costs  low, 
and  to  match  potential  operational  scenarios  for  active  sonar  tracking  of  submerged 
targets,  range-only  sensors  were  used  rather  than  bearing-only  or  bearing  and  range. 
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Figure  1-1:  Close  up  view  of  a  Clearpath  Robotic's  Kingfisher  [32],  which 
was  used  to  detect  and  track  submerged  contacts.  Additionally,  a  kingfisher 
was  used  to  simulate  a  submerged  contact  [32], 


For  this  thesis,  the  M100  Kingfisher  was  used.  Table  1.1  and  Figure  1-2  contain 
a  baseline  description  of  the  ASV.  One  of  the  benefits  of  using  the  M100  Kingfisher 
was  the  presence  of  two  large  payload  boxes  on  top  of  the  ASV.  These  boxes  afford 
the  the  ASV  the  ability  to  carry  additional  sensors.  One  important  restriction  on  the 
Kingfisher  is  that  it  is  limited  to  2  m/s  maximum  speed. 


L  x  W  x  H 

1270x1270x520mm 

Power 

200  W 

Draft 

280  mm 

Max  Speed 

2  m/s 

Weight 

34  kg 

Max  Thrust 

60  lb 

Max  Payload 

15  kg 

Operating  Time 

2  h 

Batteries 

12  V,  24  Ah 

Max  Range 

10  km 

Table  1.1:  M-100  Kingfisher  Technical  Specifications  [32] 


1.3.3  Autonomy  Software 

The  autonomy  software  used  for  this  thesis  is  a  software  suite  known  as  Mission  Ori¬ 
ented  Operating  Suite  (MOOS),  initially  developed  by  Newman  at  MIT  in  2001  [27]. 
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(a)  Rear  View 


(b)  Profile  View 


1. 


Figure  1-2:  M100  Kingfisher  [32], 


In  2004,  Benjamin  introduced  the  IvP-Helm,  a  MOOS  process  for  mission  control  that 
that  uses  Interval  Programming  (IvP)  [7]  for  multi-objective  optimization.  Many  pro¬ 
cesses  have  been  added  to  the  MOOS  structure  so  that  this  collective  software  suite 
is  now  known  as  MOOS-IvP  [8].  Further  documentation,  missions,  and  software 
regarding  MOOS-IvP  can  be  found  at  www.moos-ivp.org. 


1.4  Relevance  and  Potential  Applications 

The  development  of  autonomous  applications  able  to  track  submerged  contacts  has 
significant  potential  future  applications  for  security,  law  enforcement,  and  scientific 
research. 
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1.4.1  Harbor  Security 

One  of  the  major  security  weaknesses  in  the  United  States  is  its  lack  of  harbor  security; 
more  specifically,  the  lack  of  security  against  submerged  threats.  The  following  list 
illustrates  the  vast  and  important  role  that  ports  and  harbors  have  on  the  United 
States  and  its  economy:  [28] 

•  There  are  over  185  public  ports  in  the  United  States. 

•  Over  13.3  million  jobs  stem  from  cargo-related  spending. 

•  In  2007,  there  was  $3.95  trillion  in  international  trade  processed  in  United  States 
ports. 

•  In  2006,  1  billion  tons  of  domestic  goods  moved  via  water  in  the  United  States. 

•  Each  year,  400  million  cubic  yards  of  dredged  material  are  removed  from  navi¬ 
gation  channels  in  the  United  States. 

•  In  2008,  $400  million  was  appropriated  for  the  Department  of  Homeland  Secu- 
ritys  Port  Security. 

•  Leading  commodities  shipped  include  crude  petroleum,  petroleum  products, 
chemicals,  coal,  and  natural  gas. 

Most  of  these  ports  are  located  within  the  vicinity  of  a  major  metropolitan  area,  as 
may  be  seen  in  Figure  1-3.  By  using  a  method  similar  to  the  one  mentioned  in  this 
thesis,  harbors  may  be  made  safer  from  underwater  threats. 

1.4.2  Drug  Interdiction 

The  United  States  has  approximately  12,383  miles  of  coastline  [6];  patrolling  the 
coast  for  incoming  drug  shipments  is  a  difficult  task  for  the  LInited  States  Coast 
Guard  (USCG).  In  2006,  the  authorities  detected  three  drug-running  submersibles 
and,  later  the  same  year,  captured  a  submarine  called  Bigfoot  and  seized  several 
tons  of  cocaine.  In  2008,  an  average  of  ten  sightings  of  drug-running  submersibles  a 
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Figure  1-3:  The  top  25  U.S.  Shipping  Ports  [29].  This  maps  shows  the  annual 
amount  of  import/export  activity  that  takes  place  in  the  United  States. 

month  was  reported.  The  narco  submarine,  as  seen  in  Figure  1-4,  is  the  latest  vehicle 
of  choice  for  smuggling  drugs  into  the  United  States.  It  has  been  estimated  that  from 
October  1,  2007,  to  February  1,  2008,  that  300  tons  of  cocaine  were  smuggled  into 
the  country  using  a  narco  submarines  [24], 

The  USCG  is  unable  to  continuously  counter  this  smuggling  threat.  With  the 
purchase  and  deployment  of  a  large  number  of  inexpensive  and  reliable  AS  Vs  coupled 
with  the  tracking  algorithms  described  in  this  thesis,  the  USCG  could  significantly 
reduce  the  number  of  illegal  drugs  brought  into  the  United  States  by  means  of  the 
narco  submarines. 

1.4.3  Scientific  Research 

There  are  several  areas  of  scientific  research  in  which  autonomously  tracking  sub¬ 
merged  contacts  can  be  useful  One  immediate  need  is  for  tracking  and  monitoring 
fish  or  whale  populations.  Autonomous  tracking  would  provide  deeper  insight  into 
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Figure  1-4:  The  Narco  Submarine  [23]  was  estimated  to  have  smuggled 
around  300  tons  of  cocaine  into  the  United  States  from  October  1,  2007  to 
February  1,  2008. 


sea  life  behaviors,  migratory  patterns,  and  population  sizes.  The  technology  is  also 
needed  to  track  boundaries  of  large  masses,  such  as  the  underwater  oil  spill  from 
Deepwater  Horizon  or  the  floating  debris  held  produced  by  the  Japanese  tsunami  of 
2011. 

1.5  Research  Challenges 

The  problem  of  localizing  and  tracking  undersea  contacts  raises  many  difficult  research 
issues,  and  the  literature  in  this  held  is  vast.  Our  work  in  this  thesis  is  unique  in  several 
aspects  which  relate  to  developing  applications  to  run  in  real-time  on  an  autonomous 
marine  platform.  We  describe  the  implementation  of  a  particle  hlter  that  focuses  on 
tracking  submerged  contacts  using  applications  developed  in  MOOS-IvP,  a  powerful 
open-source  framework  for  marine  autonomy  research.  The  work  done  with  MOOS- 
IvP  so  far  has  focused  on  scientific  research  including  tracking  environmental  features 
such  as  plumes  [30]  or  performing  search  tasks  for  stationary  objects  like  mines  [33]. 
There  has  been  no  MOOS-IvP  implementation  for  localizing  and  tracking  submerged 
contacts  by  way  of  particle  hlters. 

A  second  unique  aspect  of  this  thesis  is  the  use  of  adaptive  behaviors  to  maneuver 
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the  ASV  based  on  the  particle  filter’s  estimates.  Significant  work  has  been  done  on 
adaptive  range  tracking  of  underwater  contacts  [41]  through  filters,  however  much  of 
this  work  has  yet  to  be  adapted  to  ASVs  or  AUVs.  This  thesis  leverages  previous 
work  done  on  adaptive  range  tracking  techniques  and  implements  them  on  an  ASV 
using  a  particle  filter  which  produces  a  hypothesis  of  the  contact’s  course,  speed,  and 
three-dimensional  position.  Based  on  that  information,  the  ASV  maneuvers  to  either 
intercept  or  perform  target  motion  analysis  (TMA)  to  produce  a  better  hypothesis. 

Whereas  much  of  the  research  that  has  been  done  with  particle  filter  tracking  of 
submerged  contacts  deals  with  the  use  of  bearing-only  information  [35],  our  work 
focussed  on  the  use  of  range-only  information,  which  presents  unique  challenges. 
Bearing-only  tracking  can  achieve  system  observability  by  having  three  independently 
measured  bearings  from  a  non-maneuvering  target  [17],  if  ownship’s  trajectory  has  at 
least  one  more  nonzero  derivative  than  the  target  [5],  or  ownship’s  motion  consist  of 
a  series  of  course  change  maneuvers  steadying  up  on  constant  velocity  segments  [26] . 
In  the  range-only  case,  the  question  of  maneuvering  to  achieve  observability  can  be 
more  difficult,  and  is  discussed  further  in  Chapter  4. 

Target  tracking  for  autonomous  robots  has  been  extensively  researched  for  a  va¬ 
riety  of  application  domains.  Previous  research  in  autonomous  vehicle  tracking  has 
focused  on  Unmanned  Aerial  Vehicles  (UAVs)  employing  a  fusion  of  multiple  sophis¬ 
ticated  sensors  including  LIDAR,  RADAR,  laser  range  finders,  and  visual.  However, 
the  tracking  process  is  a  significant  challenge  when  it  is  limited  to  a  single  source  of 
information,  in  this  case  range.  Range-only  information  is  difficult  to  use  for  tracking 
because  global  observability  is  not  guaranteed  [31].  Cooperation  of  multiple  track¬ 
ing  vehicles  is  an  obvious  approach  to  overcoming  limited  observability,  however  the 
problem  of  cooperative  autonomous  tracking  of  submerged  adversarial  agents  has  not 
been  extensively  researched  in  the  marine  robotics  research  community. 

Another  significant  challenge  in  this  thesis  work,  in  comparison  to  the  related 
problem  of  AUV  localization  [18,21,22],  is  the  number  of  unknown  variables.  For 
cooperative  localization  [16],  one  typically  knows  the  heading  and  depth  of  the  tar¬ 
get  vehicle,  which  are  transmitted  to  the  tracking  vehicle  via  acoustic  modems.  In 
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contrast,  in  tracking  a  non-cooperative  target,  the  course,  speed,  depth,  and  initial 
position  are  unknown. 

1.6  Thesis  Contribution 

The  primary  contribution  of  this  thesis  is  a  developed  MOOS-IvP  application  that 
is  capable  of  receiving  range  only  information  and  through  the  combination  of  TMA 
techniques  and  a  particle  filter,  can  track  a  submerged  contact  within  the  restrictions 
of  Section  1.2.  Our  work  shows  that,  under  certain  conditions,  the  particle  filter 
combined  with  TMA  techniques  was  capable  of  detecting,  localizing,  and  tracking  a 
maneuvering  contact  through  with  range-only  information. 

The  criteria  for  success  on  the  contributions  of  this  thesis  are  defined  as  the 
following: 

1.  Create  a  MOOS-IvP  software  application  that  would  allows  a  user  to  specify  a 
set  of  initial  conditions  that  enables  tracking  of  a  submerged  contact. 

2.  Conduct  a  series  of  simulation  runs  and  in- water  experiments  to  verify  the 
MOOS-IvP  software  used  by  the  ASV  is  able  to  autonomously  track  a  single 
simulated  submerged  contact. 

1.7  Thesis  Overview 

The  remainder  of  this  thesis  is  organized  by  the  following  chapters: 

Chapter  2  provides  an  overview  of  the  particle  filter  and  how  it  was  used  for  this 
thesis. 

Chapter  3  provides  an  overview  of  the  software  developed  with  MOOS-IvP. 
Chapter  4  provides  an  overview  on  the  difficulty  of  using  target  motion  analysis 
for  range-only  information. 

Chapter  5  explores  the  particle  filter’s  settings  and  the  performance  of  the  ap¬ 
plication  pParticleFilterAP  in  localizing  and  tracking  of  submerged  contacts.  This 
chapter  also  shows  the  results  from  both  the  simulation  and  a  real  in  water  test. 
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Chapter  6  presents  the  conclusions  of  this  thesis  and  discusses  potential  topics 


for  future  work. 
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Chapter  2 


Particle  Filter 


In  this  project,  a  particle  filter  was  used  and  investigated  to  determine  the  feasibility 
of  localizing  and  tracking  a  contact  using  a  singular  vehicle  with  a  single  range  only 
sensor.  A  particle  filter  was  chosen  for  its  ability  to  track  contacts  that  exhibit  non- 
Gaussian  and  nonlinear  behaviors  that  are  typical  of  sea  going  vessels.  Sea  going 
vessels  also  have  the  ability  to  maneuver,  which  can  be  detected  by  a  particle  filter 
by  observing  the  shift  in  the  weighted  particles.  A  particle  filter  is  able  to  achieve 
this  by  approximating  the  posterior  with  a  finite  number  of  parameters  [36],  in  this 
case  range.  It  does  this  by  updating  the  probability  distribution  recursively  through 
a  two-step  process.  The  first  step  happens  when  the  state  change  of  the  particle  is 
applied,  and  the  second  step,  happens  when  the  new  sensor  data  are  received  and 
used  to  update  the  weight  of  the  particles  [4], 

2.1  Particle  Filter  Overview 

Figure  2-1  shows  the  basic  structure  of  the  particle  filter  algorithm  used  in  this  project. 

The  setup  for  the  particle  filter  is  as  follows  [1,15,36].  The  first  assumption  is 
that  there  are  a  series  of  range  only  sensors  that  are  capable  of  detecting  the  contact. 
The  contact’s  state  and  particle’s  state  are  given  by: 

C t  =  [x  y  Z  6  ©  v]  (2.1) 
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Figure  2-1:  Particle  Filter  Flow  Chart:  Particles  are  initialized  on  the  onset  of 
the  first  range  measurement.  Particles  are  continually  updated  and  compared 
to  sensor  input;  based  on  this  difference  a  particle’s  weight  is  determine  based 
on  its  probability.  Once  the  combined  weight  sum  drops  below  a  certain 
threshold,  the  set  of  particles  are  resampled  with  replacement  to  form  a  new 
set  of  particles. 


where  x,y,  and  z  are  the  Cartesian  coordinates  of  the  contact,  6  is  the  contact’s  course, 
0  is  the  contact’s  pitch  angle,  and  v  represents  the  contact’s  speed.  In  this  case  the 
random  variables  are  6 ,  0,  and  v.  The  observation  is  a  vector  of  ranges  given  by: 

r\  =  [r i  r2  ...  rn]  (2.2) 

where  r\  is  the  sensed  range  from  each  of  the  i  sensors  at  time  t.  The  particle  filter 
works  by  taking  a  set  of  data,  range  in  this  case,  and  creating  a  sample  of  random 
particles  that  satisfies  the  data, 


Q~q{Q\tUrt)  (2.3) 

where  Q  represents  an  individual  particle  i  at  time  t.  In  the  case  of  tracking  a 
contact,  the  range  is  used  to  produce  particles  that  have  a  (x,y,z)  coordinate  from  a 
random  bearing  angle  and  elevation  angle.  A  random  course,  pitch,  and  speed  are 
also  generated  to  determine  the  translation  motion  of  a  particle  traveling  from  time 
t  to  t+1.  The  equations  below  show  how  the  x,  y,  and  z  coordinates  are  determined 
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from  the  random  bearing  0  and  random  elevation  angle  0. 


x  —  r  cos  0  sin  0 

(2.4) 

y  —  r  sincf)  sin  0 

(2.5) 

z  =  r  cos  0 

(2.6) 

\J  x2  +  y2  +  z2 

(2.7) 

2.2  Initialization 

Figure  2-2  shows  a  held  of  particles  of  radius  R ,  where  the  blue  points  represent 
the  particles,  the  yellow  point  represents  ownship,  and  the  red  point  is  an  example 
particle.  Since  the  only  information  that  is  given  is  range,  the  particles  are  initially 
randomly  dispersed  at  a  distance  R  from  ownship,  with  a  bearing  from  ownship  of  0, 
and  an  elevation  angle  of  0  (Algorithm  6  lines  3-4).  Initially,  both  a  course  of  d*  and 
a  speed  of  vt  are  uniformly  generated  (Algorithm  6  lines  3-4).  It  is  important  to  note 
that  the  contact  could  (but  not  necessarily)  be  one  of  the  blue  dots.  At  this  point, 
t=0,  each  particle  has  an  equal  weight  for  representing  the  belief  state. 


Algorithm  1  Particle  Filter:  Initialization 
1:  for  i  =  1  :  N  do 
2:  Sample  Q  ~  ?(Ct Kt-ibc) 

3:  Generate  Random  0* 

4:  Generate  Random  0* 

5:  Generate  Random  Course  9l 

6:  Generate  Random  Speed  vl 

7:  xl  =  rt  cos  0*  sin  0* 

8:  yl  =  rt  sin  0*  sin  0* 

9:  zl  =  rt  cos  0* 

10:  end  for 
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Figure  2-2:  Particles  are  initially  generated  from  an  initial  range  measure¬ 
ment.  The  yellow  circle  represents  ownship  and  at  a  distance  R  away  are 
particles  represented  by  blue  circles.  Each  particle  is  given  a  random  course, 
speed,  and  position  based  on  the  initial  range.  In  this  case  the  range  is  less 
than  the  bottom  depth.  In  the  case  where  bottom  depth  was  less  than  range, 
the  particle  filter  is  truncated  to  compensate  for  bottom  depth. 


2.3  Particle  Prediction 

In  each  cycle,  the  state  of  the  particle  is  predicted  based  on  the  previous  state,  course, 
and  speed.  One  important  distinction  in  the  prediction  step  is  the  addition  of  noise 
for  both  course  and  speed  variables.  To  propagate  particle  diversity  a  noise  term  was 
added  to  the  random  variables,  in  this  case  vnoise  and  9noise.  One  way  to  think  about 
this  process  is  that  the  resampling  step  disperses  the  particles  in  the  vicinity  of  the 
predicted  contact  of  interest  with  the  goal  of  obtaining  a  better  solution.  The  noise 
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is  used  to  create  this  dispersion  of  particles. 


V  V  Vnoise 


(2.8) 


0 


9  +  9r 


(2.9) 


x\+l  =  x\  +  vA t  cos  9  sin  0 


(2.10) 


ylt+1  =  y\  +  v  At  sin  9  sin  0 


(2.11) 


z\+1  =  z\  +  vAt  cos  0 


(2.12) 


Algorithm  2  Particle  Filter:  Prediction 
1:  for  i  =  1  :  N  do 

2:  V  V  Vnoise 

3:  9  =  9  +  9noise 

4:  xl  =  xl  +  v  At  cos  0  sin  0 

5:  yl  =  y%  +  v  At  sin  9  sin  0 

6:  Zl  =  Z*  COS  0 

7:  end  for 


2.4  Sensor  Input 

Particle  filters  are  capable  of  receiving  various  types  and  numbers  of  inputs.  This 
thesis  explores  using  a  particle  filter  with  only  one  range  sensor  input.  In  section  4.2.3 
multiple  range  inputs  were  used  from  multiple  vehicles  to  demonstrate  the  convergence 
of  the  particles. 
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2.5  Determination  of  Weights 


Once  the  sensor  information  was  received,  the  next  step  was  to  determine  the  weight 
of  the  particles  (importance  factor),  w.  For  each  new  range  measurement  the  weight, 
w\ ,  was  found  using  the  following  equation:  [36] 


W\  =  w\_x 


p(n\Q)p(G\Ct-i) 

9(CtlCo:t-l,0) 


(2.13) 


When  the  transition  prior  is  used  as  the  importance  function,  p(Q\Q-l)  ~  Q(Q |Co-t-l>  rt)i 
the  weight  simplihes  to  w\  =  The  weights  are  then  normalized  so 

that  J2iwi  —  1- 

The  probability  function,  p(rt\Q),  was  solved  by  understanding  and  identifying 
the  sources  of  error  for  the  range  measurement.  Three  sources  of  error  identified: 
the  first  was  range  measurement  error  produced  by  the  range  sensor,  or  in  this  case 
range  simulator.  The  second  was  the  GPS  error  since  the  position  of  the  contact 
was  determined  by  both  the  sensed  range  and  the  position  of  ownship.  The  third 
error  was  the  latency  error  from  the  vehicle’s  internal  communications  and  processes. 

Some  time  delay  existed  from  receiving  the  sensor  information  to  the  processing  of  the 
information  to  sharing  the  information  with  the  various  MOOS  modules.  These  errors 
and  delays  were  significant  enough  to  cause  error  in  the  particle  filters  prediction, 
resulting  in  the  predicted  position  being  several  meters  from  the  contact’s  position. 

By  using  the  Central  Limit  Theorem  [25],  if  Sn  is  the  sum  of  n  mutually  independent 
random  variables;  in  this  case  range  error,  location  error,  and  latency  error,  than 
Sn  can  be  approximated  as  a  Gaussian  distribution,  which  is  given  by  the  following 
equation: 


P(r«|«)  =  /«,(*)  =  (2.14) 

V27 t<j 

Figures  2-3  and  2-4  provide  illustrations  of  this  distribution.  A  floor  was  used  in  the 
Gaussian  distribution,  to  help  prevent  highly  weighted  particles  from  being  zeroed 
out  by  outliers. 
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Figure  2-3:  An  Example  Probability  Distribution  for  a  range  difference  be¬ 
tween  the  actual  sensed  range  (the  thick  black  line)  and  the  particles  (the  blue 
circles). 
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Algorithm  3  Particle  Filter:  Weight  Determination 
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for  i  =  1  :  N  do 

Calculate  Expected  Range  of  Particle 

Find  the  Range  Difference  Between  the  Expected  and  Actual,  A  Range 
Find  P(ARange) 

Weight 1  =  Weight 1  *  P(ARange) 

Overall  Weight  =  Overall  Weight + Weight1 

end  for 

NORMALIZE  WEIGHT 
for  i  =  1  :  N  do 

Weight*  =  oZl  iC* 

end  for 


fnAx) 


%/27T<T 


X 


Figure  2-4:  A  Gaussian  Distribution  was  used  to  estimate  the  probability  of 
the  range  difference  between  a  particle  and  the  actual  range  measurement. 


2.6  Resampling 

After  the  weight  of  the  particles  were  calculated,  the  particle  filter  enters  the  resam¬ 
pling  process  [1,15,36].  This  step  is  significant  because  it  helps  avoid  degeneration 
of  filter’s  estimate  (t  while  continuing  to  localize  and  track  the  contact.  To  do  this 
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the  particle  filter  resamples  the  entire  set  of  particles  once  Meff  <  Mthreshoid,  where 
Meff  =  — — 1  {L)  [19]  and  Mthreshoid  =  f.  The  particle  algorithm  draws  with  re- 

A-,L=l\wk  > 

placement  from  the  N  particles  in  the  set  Ct-  The  probability  of  drawing  each  particle 
is  determined  by  the  weight  that  was  previously  calculated.  This  resampling  trans¬ 
forms  the  set  of  N  particles  to  another  set  of  N  particles  but  with  a  new  distribution 
based  on  the  previous  weights.  This  resample  shifts  the  initial  probability  density 
function  (pdf)  to  a  pdf  that  is  more  representative  of  the  problem,  Figure  2-5  is  an 
example  of  the  shift  in  pdf. 


x 


Figure  2-5:  An  initial  PDF  was  assumed  (f(x)),  once  actual  measurements 
are  taken  the  PDF  shifts  as  a  result  of  resampling  to  a  PDF  that  reflects  the 
actually  probability  of  the  system  (g(x)). 


2.7  Particle  Filter  Momentum 


One  of  the  difficulties  in  tracking  a  maneuvering  contact  is  how  the  particle  filter 
overcomes  the  momentum  of  the  particles.  For  example,  if  a  contact  suddenly  changed 
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Algorithm  4  Particle  Filter:  Resampling 

1:  if  Meff  <  Mthreshoid  then 

2:  for  i \  —  1  :  N  do 

3:  Draw  With  Replacement  from  Q 

4:  end  for 

5:  end  if 


its  course  by  120°  the  particle  filter  would  have  a  difficult  time  adapting  to  that 
change.  Recall  that  after  each  update,  velocity  and  course  are  updated  as  follows 
v  —  v  +  vnoise  and  6  =  9  +  9noise.  If  the  course  noise  was  set  to  allow  a  deviation  of 
30°,  it  would  take  a  minimum  of  four  updates  to  have  a  particle  at  the  new  course 
at  which  the  contact  would  have  a  significant  head  start  from  the  group  of  particles. 
Once  the  particles  have  reached  the  correct  course,  the  particles  would  have  to  have  a 
higher  speed  than  the  contact  in  order  to  catch  up  to  the  contact.  Once  the  particles 
caught  up  to  the  contact  they  would  overshoot,  hence,  the  momentum  problem  for 
particle  filters  results.  Figures  2-6  and  2-7  are  examples  of  a  sharp  maneuver  where 
the  particles  do  not  follow. 

One  of  the  ways  to  overcome  the  momentum  problem  is  by  increasing  the  course 
and  speed  noise  during  the  update  step.  The  disadvantage  with  this  method  is  that 
when  the  noise  is  increased  so  is  the  associated  range  error. 

Another  method  to  mitigate  this  problem  and  the  one  incorporated  in  this  thesis 
is  the  concept  of  reserve  particles.  The  purpose  behind  reserve  particles  is  to  take  a 
small  portion  of  the  overall  set  and  use  them  to  prevent  the  momentum  problem  from 
happening.  Reserve  particles  do  this  in  the  resample  portion  of  the  algorithm.  When 
resampling  occurs  the  particles  are  drawn  from  the  same  sample  set  of  X,  Y,  and 
Z  coordinates  but  their  course  and  speed  are  generated  randomly  instead  of  drawn 
from  the  sample.  By  using  a  randomly  generated  course  and  speed  at  this  point, 
losing  track  and  overshooting  particles  are  reduced.  One  disadvantage  with  reserve 
particles  is  that  it  creates  an  offset  in  the  average  number  of  particles  as  a  result  of 
the  reserve  particles  be  dispersed  uniformly  from  the  resample  point.  For  this  reason 
it  is  important  to  limit  the  number  of  reserve  particles  to  less  than  10  percent  of  the 
overall  number  of  particles. 
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Figure  2-6:  The  particles  have  consolidated  and  grouped  around  the  target’s 
track.  If  the  contact  continues  with  its  current  course  and  speed  the  particles 
will  continue  to  track. 
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Figure  2-7:  Momentum  of  Particles:  As  the  target  maneuvers,  the  particles 
are  slow  to  regain  the  target’s  track.  The  particles  initial  continue  on  the 
previously  projected  track,  the  outliers  close  to  the  contact  slowly  gain  weight 
and  eventual  resampling  occurs  to  shift  the  particles  closer  to  the  actually 
track.  If  the  particles  deviated  too  far  from  the  track  they  may  not  reconverge 
on  the  contact  and  instead  converge  on  track  with  a  parallel  solution. 
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Chapter  3 


Software  Architecture  and  Modules 


The  principle  work  developed  by  this  project  was  done  using  MOOS-IvP.  A  particle 
filter  software  module  was  created  (pParticleFilterAP)  to  hypothesize  possible  con¬ 
tacts’  course,  range,  and  position  information.  These  hypotheses  were  sent  to  the 
IvP  Helm  CutRange  Behavior  which  maneuvered  the  ASV  autonomously  in  order  to 
track  the  contact  of  interest. 


3.1  MOOS 

One  advantage  of  using  MOOS-IvP  as  a  software  suite  is  that  it  is  platform- independent, 
meaning  that  it  can  be  run  on  any  number  of  ASVs,  AUVs,  or  other  autonomous  ve¬ 
hicles.  MOOS  is  structured  to  act  as  a  communicator  for  different  processes:  each 
process  represents  a  different  application,  component,  or  attribute  of  the  Autonomous 
Vehicle  (AV).  MOOS-IvP  has  several  central  applications  that  are  open-source  and 
platform-independent  but  also  allows,  the  user  has  the  option  of  extending  the  soft¬ 
ware  by  writing  new  applications.  Inter-process  communication  in  a  community  of 
MOOS  applications  flow  through  a  central  MOOS  Database  (MOOSDB).  Each  pro¬ 
cess  is  independent  of  one  another  and  they  share  their  information  with  MOOSDB. 
The  processes  share  information  by  publishing  a  (Message  Key,  Message  Value)  pair 
to  the  MOOSDB.  Other  processes  are  able  to  request  information  from  the  MOOSDB 
structure  by  subscribing  for  needed  information.  Figures  3-1  through  3-4  illustrate 
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how  several  MOOSDB  communities  exist  in  order  to  perform  different  tasks  and  com¬ 
municate  with  one  another.  Figure  3-1  shows  that  the  individual  AVs  communicate 
directly  to  the  shoreside.  The  shoreside  MOOS  community  receives  these  messages 
via  the  pMOOSBridge  application  and  posts  the  messages  to  the  local  shoreside 
MOOSDB.  This  communication  is  done  through  WiFi  connectivity.  One  important 
aspect  is  that  each  MOOSDB  community  is  located  locally,  meaning  that  the  AVs’ 
communities  are  located  on  the  computers  within  the  AVs  and  that  the  shoreside 
community  is  located  on  a  computer  shoreside.  For  the  test,  the  submerged  contact 
was  simulated  on  a  kingfisher. 


Figure  3-1:  MOOS  Shoreside  to  Multi-Vehicle  Topology:  Three  vehicles 
are  deployed  with  each  vehicle  maintaining  a  baseline  connectivity  with  the 
shoreside  command  and  control  through  WiFi  communications.  Each  vehicle 
node  and  shoreside  node  are  comprised  of  a  dedicated  MOOS  community.  In 
our  case  only  two  vehicles  were  present,  leaving  the  need  to  simulate  one.  [9] 


Figures  3-2  through  3-4  were  the  simulated  MOOS  communities  used  for  this 
project. 
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Figure  3-2:  MOOS  Shoreside  Community:  This  is  an  example  of  the  simu¬ 
lated  community  used  for  the  shoreside. 


Figure  3-3:  MOOS  ASV  Community:  This  is  an  example  of  the  simulated 
community  used  for  the  ASV.  pParticleFilterAP  was  the  application  used  for 
localizing  and  tracking  the  contact. 
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Figure  3-4:  MOOS  Submerged  Contact  Community:  This  is  an  example  of 
the  simulated  community  used  for  the  submerged  contact. 


The  following  are  descriptions  of  the  MOOS  Modules  [11]  shown  in  Figures  3-2 
through  3-4; 

•  MOOSDB  is  the  central  database  used  for  MOOS  modules. 

•  pLogger  is  a  process  that  records  the  publications  of  applications  involved  in  a 
MOOS  session. 

•  pHclmlvP  is  a  behavior-based  autonomous  decision-making  MOOS  application. 
It  consists  of  a  set  of  behaviors  reasoning  over  a  common  decision  space  such  as 
the  vehicle  heading  and  speed.  Behaviors  are  reconciled  using  multi-objective 
optimization  with  the  Interval  Programming  (IvP)  model.  It  publishes  infor¬ 
mation  such  as  desired  heading,  speed,  and  depth  to  drive  the  vehicle. 

•  uSimMarine  is  a  simple  3D  vehicle  simulator  that  updates  vehicle  state,  position 
and  trajectory,  based  on  the  present  actuator  values  and  prior  vehicle  state. 
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Typical  usage  scenario  has  a  single  instance  of  uSimMarine  associated  with 
each  simulated  vehicle. 

•  pNodeReporter  is  a  simple  MOOS  app  for  collecting  node  information  such  as 
present  vehicle  position,  trajectory  and  type,  and  posting  it  in  a  single  report 
for  sharing  between  vehicles  or  sending  to  a  shoreside  display. 

•  pEchoVar  is  tool  that  may  be  used  to  subscribe  for  a  variable  and  re-publish  it 
under  a  different  name.  It  also  may  be  used  to  pull  out  certain  fields  in  string 
publications  consisting  of  comma-separated  parameter=value  pairs,  publishing 
the  new  string  using  different  parameters. 

•  pMarinePID  is  an  application  that  provides  proportional-integral-derivative  con¬ 
trol  for  vehicles  parameters  such  as  depth-pitch,  speed-thrust,  and  heading- 
rudder. 

•  uTimerScript  allows  the  user  to  script  a  set  of  pre-conhgured  pokes  to  a  MOOSDB 
with  each  entry  in  the  script  happening  after  a  specified  amount  of  time.  The 
script  may  be  paused  or  fast-forwarded.  Events  may  also  be  configured  with 
random  values  and  happen  randomly  in  a  chosen  window  of  time. 

•  uFldContactRangeSensor  is  typically  run  in  a  shoreside  MOOS  community.  It 
takes  reports  from  remote  vehicles  and  notes  their  position.  It  takes  a  range 
request  from  a  remote  vehicle  and  returns  a  range  report  indicating  that  ve¬ 
hicle’s  range  to  nearby  vehicles.  Range  requests  may  or  may  not  be  answered 
dependent  on  inter-vehicle  range.  Reports  may  also  have  noise  added  to  their 
range  values. 

•  uProcessWatch  monitors  the  presence  of  MOOS  processes  by  subscribing  to  and 
identifying  changes  in  clients  that  are  connected  to  MOOSDB. 

•  pMarineViewer  is  a  GUI  tool  for  rendering  events  in  an  area  of  vehicle  operation. 
It  repeatedly  updates  vehicle  positions  from  incoming  node  reports,  and  will 
render  several  geometric  types  published  from  other  MOOS  apps.  The  viewer 
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may  also  post  messages  to  the  MOOSDB  based  on  user-configured  keyboard  or 
mouse  events. 

•  pMOOSBridge  is  used  to  communicate  information  between  distinct  MOOS 
communities.  These  communities  may  be  all  on  the  same  machine  or  distributed 
on  network. 

•  pParticleFilterAP  generates  particles  that  are  used  to  localize  and  track  con¬ 
tacts. 


3.2  IvP  Helm 


Behaviors  Objective  Functions 


Solver 


Result 


Figure  3-5:  Action  Selection  with  Interval  Programming:  Each  active  behav¬ 
ior  produces  an  objective  function.  The  objective  functions  feed  into  a  solver 
which  handles  multiple  object  functions  from  each  behavior  by  multi-objective 
optimizations  using  the  interval  programming  method.  [8] 
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The  IvP  Helm  [11]  is  a  behavior-based  autonomous  helm  implemented  as  a  MOOS 
application. 

It  is  based  on  Interval  Programming  [8]  where  each  active  behavior  produces  a 
piecewise  linearly  defined  objective  function.  On  each  iteration  of  the  Helm,  the  IvP 
solver  (Figure  3-5)  solves  a  multi- objective  optimization  problem  from  the  collection 
of  weighted  objective  functions  from  contributing  behaviors.  The  result  on  each 
iteration,  is  a  requested  heading,  speed  and  depth  to  be  published  to  the  MOOSDB. 
Figure  3-5  demonstrates  the  flow  path  from  the  initial  behavior  to  the  maneuver.  The 
red  areas  represent  the  optimal  courses  and  speeds  based  on  the  objective  functions. 

3.3  Range  Sensor  Information 

A  simulated  off-board  range  measurement  is  produced  through  the  application  uFld- 
ContactRangeSensor  [9] .  The  parameters  for  this  module  are  shown  in  Configuration 
Block  5.  The  range  measurements  produced  by  this  module  are  based  on  the  ground 
truth  value  with  additive  noise.  For  our  work  in  this  project,  additive  noise  with 
either  a  uniform  or  a  Gaussian  error  distribution  was  utilized. 

This  application  produces  a  range  report  with  the  following  format  that  is  read 
by  the  particle  filter: 

CRS  RANGE  REPORT  =vname=archie,range=23.4,target=jackal,time=2342551.213” 

•  Line  8:  ping.wait  is  the  time  delay  between  range  pulses. 

•  Line  13:  rn  algorithm  is  the  amount  of  error  introduced  to  ground  truth.  In 
this  case  the  error  that  was  introduced  was  from  a  uniform  distribution,  which 
has  a  maximum  error  of  4  percent  of  the  actual  range  measurement. 


3.4  pParticleFilterAP 

This  section  provides  guidance  for  how  to  operate  the  particle  filter  settings  developed 
for  this  thesis.  The  algorithms  and  descriptions  used  within  the  particle  filter  are 
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Configuration  Block  5  uFldContactRangeSensor  Settings 
1:  ProcessConfig  =  uFldContactRangeSensor 
2:  { 

3:  AppTick  =  4 
4:  CommsTick  =  4 
5: 

6:  reply  ^distance  =  jackal  =  50 
7:  reach-distance  =  archie  =  390 
8:  ping.wait  =  archie  =  5 
9:  report_vars  =  both 
10:  ping-color  =  white 
11:  reply-color  =  chartreuse 
12:  verbose  =  true 

13:  rn_algorithm  =  uniform, pct=0. 04 
14:  } 


found  in  Chapter  2.  Configuration  Block  6  is  one  possible  way  to  configure  the 
pParticleF  ilter  AP. 


3.4.1  Configuration  Parameters  for  pParticleFilterAP 

•  Line  6:  XYZ_REPORT  is  used  to  obtain  local  AV  information  such  as  course, 
speed,  position,  and  time.  When  a  collaborative  contact  is  used,  this  report 
is  also  used  to  get  the  collaborator’s  time  and  position  to  use  with  the  range 
report. 

•  Line  7:  RANGE-REPORT  is  used  to  extract  both  the  ownship’s  range  report 
and  collaborative  ship’s  range  report. 

•  Line  8:  MY.SHIP  used  to  identify  the  ownship. 

•  Line  9:  MY -FRIEND  used  to  identify  the  collaborating  ship. 

•  Line  10:  MY  .CONTACT  used  to  identify  the  contact  that  is  being  tracked. 

•  Line  11:  MY-BEST-GUESS  and  Line  12:  MYWVG-GUESS  are  solutions  gen¬ 
erated  for  the  use  of  the  CutRange_BHV.  A  discussion  of  these  two  parameters 
is  located  in  Section  5.1.1 
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Configuration  Block  6  Particle  Filter  Settings 
1:  ProcessConfig  =  pParticleFilterAP 
2:  { 

3:  AppTick  =  4 
4:  CommsTick  =  4 
5: 

6:  XYZ .REPORT  =  NODE.REPORT 
7:  RANGE.REPORT  =  CRS.RANGE.REPORT 
8:  MY  .SHIP  =  archie 
9:  MY  FRIEND  =  betty 
10:  MY  .CONTACT  =  jackal 
11:  MY.BEST.GUESS  =  besttarget 
12:  MY_AVG_GUESS  =  avgtarget 
13:  MAX.SPEED  =  2 
14:  MAX.DEPTH  =  30 
15:  SPEED  .NOISE  =  0.5 
16:  COURSE.NOISE  =  10 
17:  RANGE.VAR  =  10 
18:  NO  .PARTS  =  2000 
19:  N.THRESHOLD  =  1000 
20:  DISPLAY  .PARTS  =  YES 
21:  RESERVE.PARTS  =  300 
22:  CEILING  =  4 
23:  FLOOR  =  100 
24:  STORE.DATA  =  NO 
25:  } 
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•  Line  13:  MAX_SPEED  is  the  maximum  possible  speed  of  the  contact.  It  is  used 
to  limit  the  particle  space  (section  5.1.2). 

•  Line  14:  MAX_DEPTH  is  the  maximum  possible  depth  of  the  contact  based  on 
geographic  and  vehicle  constraints.  It  is  used  to  limit  the  particle  space  (section 
5.1.2). 

•  Line  15:  SPEED  .NOISE  is  the  amount  of  speed  noise  introduced  in  the  pre¬ 
diction  step  (section  2.3).  This  number  is  multiplied  by  a  uniform  random 
distribution  [0,1]. 

•  Line  16:  COLIRSE.NOISE  is  the  amount  of  course  noise  introduced  in  the 
prediction  step  (section  2.3).  This  number  is  multiplied  by  a  uniform  random 
distribution  [0,1]. 

•  Line  17:  RANGE.VAR  is  used  as  the  variance  in  the  weight  evaluation  in 
Section  2.5. 

•  Line  18:  NO.PARTS  is  the  number  of  particles  used.  2000  was  found  to  be  an 
acceptable  number.  If  this  number  is  reduced,  the  error  is  increased  as  a  result 
of  decreasing  the  chances  of  finding  the  solution.  If  this  number  is  increased,  so 
are  the  required  computer  resources. 

•  Line  19:  N.THRESHOLD  helps  determine  the  point  at  which  the  particles  are 
resampled  based  on  overall  weight  (section  2.6).  Typical  values  are  usually  half 
of  NO.PARTS. 

•  Line  20:  DISPLAY  .PARTS  is  used  to  displays  particles  with  pMarineViewer. 
This  does  not  work  well  is  the  number  of  particle  is  greater  than  200. 

•  Line  21:  RESERVE-PARTS  is  the  number  of  reserve  particles  used.  This  is 
discussed  in  section  2.7. 

•  Line  22:  CEILING  is  a  ceiling  used  for  the  Gaussian  distribution  calculation 
in  section  2.5.  This  is  needed  to  avoid  situations  in  which  wrong  solutions 
outweigh  correct  solutions  based  on  minor  range  errors. 
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•  Line  23:  FLOOR  is  a  floor  used  for  the  Gaussian  distribution  calculation  that 
was  discussed  in  Section  2.5. 

•  Line  24:  STORE  DATA  is  used  to  store  particle  and  the  best  hypothesis  infor¬ 
mation.  The  stored  date  is  located  in  data.txt  with  the  following  format:  (Time, 
Ownship  Position  [X,Y,Z],  Ownship  Course,  Ownship  Speed,  Contact  Position 
[X,Y,Z],  Contact  Course,  Contact  Speed,  Particle  1  Position  [X,Y,Z],  Particlel 
Course,  Particle  1  Speed,  ....  , Particle  N  Position  [X,Y,Z],  Particle  N  Course, 
Particle  N  Speed,  Particle  Best  Position  [X,Y,Z],  Particle  Best  Course,  Particle 
Best  Speed,  Particle  AVG  Position  [X,Y,Z],  Particle  AVG  Course,  Particle  AVG 
Speed  ). 

3.4.2  MOOS  Variables  Posted  by  pParticleFilterAP 

The  primary  output  of  pParticleFilterAP  to  the  MOOSDB  are  the  following  node 

reports  needed  for  the  CutRange  Behavior. 

•  NODE_REPORT  publishes  the  highest  weighted  particle  (MY_BEST_GLTESS) 
and  the  average  weight  of  the  particle  (MY_BEST_GUESS).  This  is  used  in 
behaviors  like  BHV  CutRange. 

3.4.3  MOOS  Variables  Subscribed  by  pParticleFilterAP 

Variables  subscribed  for  pParticleFilterAP  are  summarize  below. 

•  XYZ .REPORT  (NODE  REPORT)  is  the  used  to  get  ownship  and  collaborative 
ships’  attributes  such  as  course,  speed,  and  position  information. 

•  RANGE_REPORT  (CRS_RANGE_REPORT)  is  used  to  get  range  reports  from 
ownship  and  collaborative  ships. 
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3.5  The  CutRange  Behavior 


The  CutRange  Behavior  (Configuration  Block  7)  acts  to  reduce  the  range  from  own- 
ship  to  a  specified  vehicle  given  by  a  node  report.  In  the  case  of  this  thesis,  the 
node  report  that  is  generated  is  based  on  either  the  average  particle  position  or  the 
highest-weighted  particle  position.  Line  13  PATIENCE:  Can  either  be  0  or  100:  0 
represents  an  straight  course  to  the  contact’s  present  position  and  100  represents  a 
course  along  the  path  to  CPA. 


Configuration  Block  7  CutRange  Settings 
1:  ProcessConfig  =  III IV  CutRange 
2:  { 

3:  AppTick  =  4 
4:  CommsTick  =  4 
5: 

6:  name  =  trackbest 
7:  pwt  =  90 

8:  condition  =  RETURN  =  false 
9:  condition  =  (DEPLOY  =  true) 

10:  PWT  OUTER  DIST  =  20 

11:  PWT  INNER  DIST  =  2 

12:  GIVEUP_DIST  =  800 

13:  PATIENCE  =  0 

14:  EXTRAPOLATE  =  true 

15:  ON_NO_CONTACT_OK  =  true 

16:  TIME_ON_LEG  =  60 

17:  CONTACT  =  besttarget 

18:  } 


3.6  Overall  Software  Organization 

Information  flows  from  the  range  sensor  to  the  Particle  Filter.  The  Particle  Filter 
communicates  with  the  TMA  function  (Chapter  4)  to  determine  the  best  maneuver 
and  sends  that  information  to  the  CutRange  Behavior.  This  behavior  then  influences 
the  course  chosen  in  the  IvP  Helm. 
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Figure  3-6:  Software  Flow  Chart:  The  main  applications  used  in  this  thesis 
are  the  simulated  range  sensor,  the  particle  filter  with  TMA  decisions,  and  the 
cut  range  behavior.  The  particle  filter  receives  input  from  the  simulated  range 
sensors  and  produce  output  to  the  cut  range  behavior.  A  similar  structure  can 
be  found  in  [41]  pg.  303. 
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Chapter  4 


Target  Motion  Analysis 


This  chapter  discusses  the  problem  caused  from  the  lack  of  observability  encountered 
when  using  range-only  measurements.  A  bearing-only  measurement  solution  can  be 
derived  through  a  series  of  maneuvers  [5,14,17,26];  however,  there  are  more  limitations 
for  finding  system  observability  of  range-only  measurement  target  tracking  [34] .  This 
lack  of  global  observability  places  restrictions  and  guidelines  on  how  to  localize  a 
contact.  These  guidelines  are  discussed  throughout  this  chapter. 


4.1  Observability  Analysis 


For  simplicity  it  was  assumed  that  the  depth  of  the  submerged  contact  was  constant, 
thereby  reducing  the  state  Equation  4.2  from  nine  variables  to  six  variables.  Figure 
4-1  references  the  relative  motion  used  for  these  equations.  To  determine  if  the  system 
of  range-only  measurements  is  observable,  the  following  equations  [34]  are  used: 


n  =  An  +  BAm 
n=(X,Y,X,Y,ATx,ATy)T 
Am  ( Arnx ,  AmY ) 
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(4.1) 

(4.2) 

(4.3) 


0  I2  0 


(4.4) 


A 


0  0  I2 
0  0  0 


B  = 


0 

-h 

0 


h  = 


1  0 
0  1 


a  R  =  ^x(tly  +  Y(tly 


P 


tan  (— ) 


(4.5) 

(4.6) 

(4.7) 

(4.8) 


Since  the  value  of  (3  is  unknown,  the  contact’s  position  can  not  be  directly  solved; 
this  causes  the  ambiguity  that  is  shown  through  time  progression  in  Figure  4-2 
through  Figure  4-5.  This  ambiguity  can  lead  to  non-convergence  of  the  contact. 
Toward  the  end  of  this  progression  more  particles  are  eliminated;  however,  several 
still  remain  due  to  the  infinite  number  of  course/speed  solutions  demonstrated  in 
Equations  4.7  and  4.8  above. 
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Figure  4-1:  Relative  Motion:  Ownship  travels  at  a  speed  of  Vownship  while 
the  contact  travels  at  a  speed  of  Vcontact ■  The  two  important  variables  needed 
to  be  solved  for  the  observability  analysis  is  (5  and  Vneiauve- 
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Range  Rings  in  100  meter  increments 
Contact’s  Range 
Contact’s  Track 
Ownship  (USV) 

New  Particle 
Low  Weight  Particle 
High  Weight  Particle 


345°  15° 


Figure  4-2:  Relative  Motion  Plot  of  Particles  at  t  =  0.  This  plot  shows  the 
initialization  of  the  10  particles,  each  with  different  courses,  speeds,  and  initial 
bearings  from  ownship. 


Particle 

Course  (°) 

Speed  meters, 

^  second 

Weight 

1 

225° 

0.707 

0.10 

2 

340° 

1.068 

0.10 

3 

210.5° 

1.300 

0.10 

4 

0° 

1.000 

0.10 

5 

137.5° 

1.356 

0.10 

6 

235.5° 

0.610 

0.10 

7 

OO 

o 

o 

0.104 

0.10 

8 

191.9° 

1.272 

0.10 

9 

9.9° 

1.436 

0.10 

10 

135° 

0.342 

0.10 

Target 

337° 

1.256 

N/A 

Range  Rings  in  100  meter  increments 
Contact’s  Range 
Contact’s  Track 
Ownship  (USV) 

New  Particle 
Low  Weight  Particle 
High  Weight  Particle 


345°  15° 


Figure  4-3:  Relative  Motion  Plot  of  Particles  at  t  =  1:  After  the  first  time 
step  four  of  the  ten  particles  have  matching  relative  motion.  Notice  that  each 
of  the  four  particles  have  different  starting  positions,  courses,  and  speeds. 


Particle 

Course  (°) 

Speed  meters, 

^  second 

Weight 

1 

225° 

0.707 

0.068 

2 

340° 

1.068 

0.002 

3 

210.5° 

1.300 

0.182 

4 

0° 

1.000 

0.182 

5 

137.5° 

1.356 

0.068 

6 

235.5° 

0.610 

0.02 

7 

OO 

o 

o 

0.104 

0.068 

8 

191.9° 

1.272 

0.182 

9 

9.9° 

1.436 

0.182 

10 

135° 

0.342 

0.068 

Target 

337° 

1.256 

N/A 
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Range  Rings  in  100  meter  increments 
Contact’s  Range 
Contact’s  Track 
Ownship  (USV) 

New  Particle 
Low  Weight  Particle 
High  Weight  Particle 


Particle 

Course  (°) 

CJnrrrl  meters 

Weight 

kJ  ULLU.  j 

r  second 

3 

210.5° 

1.300 

0.195 

4 

0° 

1.000 

0.073 

8 

191.9° 

1.272 

0.195 

9 

9.9° 

1.436 

0.195 

Target 

337° 

1.256 

N/A 

Figure  4-4:  Relative  Motion  Plot  of  Particles  at  t  =  2:  As  time  goes  on 
more  particles  can  be  eliminated  through  weight  calculation;  however,  several 
particles  still  exist  as  possible  hypothesis’s  for  the  contact. 
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Range  Rings  in  100  meter  increments 
Contact’s  Range 
Contact’s  Track 
Ownship  (USV) 

New  Particle 
Low  Weight  Particle 
High  Weight  Particle 


Particle 

Course  (°) 

Speed  meters, 

^  second, 

Weight 

3 

210.5° 

1.300 

0.216 

8 

191.9° 

1.272 

0.216 

9 

9.9° 

1.436 

0.216 

Target 

337° 

1.256 

N/A 

Figure  4-5:  Relative  Motion  Plot  of  Particles  at  t  =  3:  Multiple  hypotheses 
remain  even  after  the  contact  transitions  from  a  closing  aspect  to  an  open 
aspect.  The  three  remain  particles  transition  from  a  closing  to  an  opening  at 
the  same  time.  This  demonstrates  a  need  to  disambiguate  the  particles  to 
find  the  contacts  solution. 
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4.2  Disambiguating  the  Particle  Filter 

There  are  four  distinct  ways  to  provide  convergence  to  the  particle  filter  [31,34,39,40]: 

•  Know  the  initial  state  of  the  contact. 

•  Add  a  non-range  dependent  sensor  (i.e.  bearing)  to  the  USV  that  allows  for 
cross-correlation  between  sensors. 

•  Provide  an  additional  range  sensor  from  a  different  location  and  apply  the  new 
range  information  to  the  particle  filter.  This  could  be  done  through  the  collab¬ 
oration  of  additional  US  Vs. 

•  Maneuver  the  USV  to  disambiguate  from  the  other  previously  feasible  particles 

[34]. 

4.2.1  Knowledge  of  Initial  State 

Knowledge  of  the  initial  state  can  be  achieved  by  either  observing  or  by  making 
assumptions  in  certain  geographic  situations.  For  example,  it  could  be  estimated  that 
a  contact  is  traveling  at  typical  transit  speed,  and  if  that  contact  enters  a  channel  or 
choke  point  an  initial  state  can  be  deduced. 

4.2.2  Additional  Sensor 

Adding  another  sensor  like-bearing  allows  for  the  vehicle  to  disambiguate  the  particles 
and  find  the  contact  of  concern  through  sensor  correlation  (Figure  4-6). 

4.2.3  Range  Correlation 

Another  method  for  particle  filter  correction  is  through  fusing  range  information  from 
another  source.  Figures  4-7  and  4-8  show  how  in  two  time  steps  two  vehicles  sharing 
range  information  can  localize  the  contact.  Figure  4-7  is  the  first  time  step.  The 
particles  are  formed  at  the  intersection  of  the  two  range  circles.  At  the  second  time 
step  Figure  4-8  one  of  the  particle  clusters  was  eliminated  due  to  the  distance  from  the 
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Figure  4-6:  Sensor  Correlation:  One  of  the  ways  to  remove  hypothesis  am¬ 
biguity  is  through  correlated  range  measurements  with  other  sensor  measure¬ 
ments,  in  this  case  bearing.  With  both  range  and  bearing  information,  the 
contact  can  be  localized. 


actual  range  measurements.  This  method  demonstrates  the  effectiveness  of  multiple 
vehicles  sharing  range  information  and  the  speed  at  which  ambiguity  was  removed; 
an  additional  demonstration  of  this  is  shown  [42], 


Variable: fiva  Time:[iva  Value:  |To  add  Scope  Variables  SCOPE ■VARNAME  In  ttvs  MOOS  config  block 


Figure  4-7:  Range  Fusion  at  t=0:  Initially  the  two  collaborating  vehicles 
have  two  clusters  of  particles  for  the  contacts  hypothesized  position.  The  red 
circles  are  range  circles  where  the  particles  are  generated  at  the  intersections 
of  the  shared  range  circles  for  each  vehicle. 
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Figure  4-8:  Range  Fusion  at  t=l:  At  the  next  time  step  the  two  collaborating 
ASVs  were  able  to  eliminate  one  of  the  clusters.  This  result  is  true  as  long  as 
the  collaborating  vehicles  do  not  have  zero  relative  motion  with  one  another. 


4.2.4  Adaptive  Maneuvering 

Recall  that  from  Equations  4.7  and  4.8  that  the  state  of  range  only  measurements 
were  not  observable.  Using  Modified  Polar  Coordinates  (MPC)  and  the  associated 
transformations  -in  which  at]  are  arbitrary  constants  where  at  least  one  constant  is 
nonzero  [34]-the  following  conditions  for  local  observability  were  obtained: 

f A(t)\  ^  f an  +  <3’i2 At  +  a^At2 

yU  ( t )  J  ya2i  +  a22  At  +  023  At2 

Equation  4.9  implies  that  if  the  relative  bearing  between  ownship  and  the  contact 
is  constant,  or  if  ownship  is  traveling  at  a  constant  velocity,  or  ownship  is  travel¬ 
ing  at  a  constant  acceleration,  the  system  is  unobservable;  however,  if  the  contact’s 
acceleration  is  zero,  the  Equation  4.9  reduces  to  the  following: 


f  X® 


7^ 


(Oil  +  di2  At 
a21  +  a,22At 


(4.10) 
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Equation’s  4.10  observability  requirement  is  violated  if  both  ownship  and  the  contact 
maintain  a  constant  track.  Equation  4.10  also  requires  that  ownship  maintain  a 
constant  velocity.  From  these  conditions  two  types  of  adaptive  maneuvers  were  used 
to  disambiguate  particles.  The  first  is  the  initialization  maneuver.  The  initialization 
maneuver  performs  a  loiter  pattern  (Figure  4-9)  upon  detection  of  the  contact.  This 
ensures  that  Equation  4.10  becomes  observable.  It  is  important  to  note  that  this 
maneuver  commences  with  the  initialization  of  the  particle  filter. 
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Figure  4-9:  Initial  Localization  Maneuver  through  Loitering:  This  is  done  to 
remove  the  ambiguous  hypotheses  by  changing  the  relative  motion  between 
ownship  and  the  contact.  Ownship  performs  each  turn  after  a  series  of  range 
measurements  to  ensure  enough  information  is  obtained  to  promote  differences 
in  the  relative  motions  of  the  ambiguous  hypotheses. 


The  second  adaptive  maneuver  that  was  used  was  the  intercept  maneuver.  The  in¬ 
tercept  maneuver  (Figure  4-10)  uses  a  built-in  MOOS  behavior  known  as  BHV -Outrange. 
The  behavior  acts  to  reduce  the  range  between  ownship  and  the  contact  through  ei¬ 
ther  pursuing  the  contact’s  node  report  position  or  the  Closest  Point  of  Approach 
(CPA)  with  the  contact.  Because  the  particles  are  constantly  fluctuating  due  to  the 
introduction  of  noise,  these  course  changes  provide  enough  relative  velocity  to  dis- 
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ambiguate  the  possible  solutions  that  are  generated  through  the  degradation  of  the 
particles.  If  after  a  certain  amount  of  time  ownship’s  course  remains  constant,  an 
offset  is  added  to  the  particle  solutions  (highest  and  average  weight)  to  ensure  that 
ownship  does  not  maintain  a  constant  track. 
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Figure  4-10:  Intercepting  Maneuver:  Once  the  contact  has  been  localized, 
ownship  uses  BHV .Outrange  to  pursue  two  projected  hypotheses,  the  high¬ 
est  weighted  and  the  average  particle  position.  If  ownship’s  course  has  not 
changed  in  several  range  measurements,  a  45°  offset  is  added  to  the  average 
particle  position.  This  is  done  to  remove  the  possible  loss  of  observability  from 
a  constant  relative  motion. 


The  intercept  maneuver  prosecutes  two  different  solutions.  The  first  solution  is 
based  on  the  highest  weighted  particle  (Figure  4-11),  while  the  second  solution  is  the 
average  of  the  particles  (Figure  4-12).  These  solutions  are  used  in  the  BHV .Outrange, 
HelmlvP  (Figure  4-13)  which  uses  these  to  find  the  most  appropriate  course  to  pur¬ 
sue. 


66 


IvPFunction-Viewer 


File  Rotate/Zoom  Color-Map  Behaviors 


":lfl,che 


Figure  4-11:  Visualization  of  IvPHelm  using  B HV -Outrange  with  the  High 
est  Weighted  Particle  as  the  target. 


Figure  4-12:  Visualization  of  IvPHelm  using  B HV -Outrange  with  the  Av 
erage  Weighted  Particle  Position  as  the  target. 
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Figure  4-13:  Visualization  of  IvPHelm  using  the  collective  of 

BHV -Outrange  behaviors. 


Chapter  5 


Simulations,  Testing,  and  Analysis 


This  chapter  discusses  some  of  the  particle  filter  settings  and  their  effectiveness  on 
solution  convergence,  the  simulations  that  were  conducted,  and  the  in  water  Charles 
River  test.  The  results  show  in  simple  non-maneuvering  or  slightly  maneuvering  cases 
that  the  method  developed  for  tracking  was  capable  to  tracking  a  submerged  contact. 
As  the  contact  develops  more  complicated  patterns,  tracking  because  more  difficult 
requiring  concepts  like  reserve  particles  to  be  developed  and  implemented. 


5.1  Particle  Filter  Settings 

5.1.1  Measures  of  Effectiveness 

To  determine  the  effectiveness  of  the  particle  filter,  metrics  were  established  to  cal¬ 
culate  the  distance  between  the  particles  and  the  true  position  of  the  static  contact. 
For  this,  three  methods  were  used  to  estimate  the  position  of  the  contact  used.  [1,37] 

1. )  The  first  method  was  to  estimate  the  position  of  the  contact  by  using  the 
particle  with  the  highest  weight  (the  green  triangle). 

2. )  The  second  method  was  to  find  the  average  position  of  the  particles.  A  straight 
average  is  used  even  though  it  was  not  a  good  initial  estimate  (Figure  5-1).  Once 
the  initialization  is  complete,  the  group  of  particles  centralize  to  one  cluster  and  the 
average  becomes  a  good  estimate.  Initially  the  average  weight  (the  red  circle)  is 
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located  in  the  middle  of  the  particle  spread,  not  neighboring  any  particles  due  to  the 
initial  formation  of  particles  along  the  range  circle. 

3.)  The  third  method  was  to  determine  the  weighted  average  of  the  particles. 
Testing  showed  this  method  was  the  most  accurate  but  had  results  very  similar  to 
the  average  of  the  particles. 


Average  Weight  of  Highest  Bin 


Average  Weight 


Highest  Weight 


Figure  5-1:  Initial  Particle  Separation  for  a  Stationary  Contact.  Several 
ambiguous  clusters  are  formed  after  the  resample  step.  It  is  important  to 
note  that  the  average  weight  wast  used  as  a  metric  despite  its  divergence  at 
initialization.  The  average  weight  of  the  most  populated  cluster  is  shown  in 
yellow,  after  a  few  time  steps  the  average  weight  and  the  average  weight  of 
the  most  populated  cluster  are  the  same. 


5.1.2  Number  of  Particles 

One  question  that  arises  from  the  use  of  a  particle  filter  is:  How  many  particles  are 
necessary  in  order  to  find  a  solution  in  a  reasonable  amount  of  time.  If  the  filter  uses 
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a  larger  number  of  particles,  the  time  needed  to  find  a  reliable  solution  is  decreased 
but  the  overall  computational  power  that  was  expended  is  increased.  Conversely,  the 
fewer  number  of  particles  used,  the  more  time  required  to  find  a  solution.  To  evaluate 
this  trade  off,  a  series  of  simulations  was  run  to  observe  the  trade  space  between  the 
number  of  particles  and  the  time  required  for  solution  convergence. 

Establishing  the  resolution  needed  for  the  solution  and  the  range  of  the  key  vari¬ 
ables  are  also  vital  aspects  for  determining  the  number  of  particles  required  for  con¬ 
vergence.  This  implementation  of  the  particle  filter  contains  the  following  variables 
and  their  associated  ranges  (Table  5.1). 


Variable 

Range 

Max  Depth 

Bearing 

Course 

Speed 

0-30  meters 

0-360° 

0-360° 

0  iometers 

w  second 

Table  5.1:  Table  of  Variables 


Figure  5-2  shows  the  number  of  time  steps  needed  for  convergence  versus  the 
number  of  particles  used.  Several  trials  were  conducted  with  fewer  than  50  particles 
and  all  failed  to  converge.  For  the  purposes  of  this  thesis,  2,500  particles  were  used 
for  all  of  the  follow-up  scenarios. 
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Convergence  Rate  of  Particles 


0  500  1000  1500  2000  2500  3000  3500  4000  4500  5000 

Number  of  Particles 

Figure  5-2:  Range  Convergence  for  the  number  of  particles  over  time.  At 
some  point  increasing  the  number  of  particles  does  not  significantly  improve 
the  rate  of  convergence.  2500-3000  particles  appear  to  be  the  optimal  number 
of  particles  for  this  project. 


5.1.3  Moving  Contact  Settings 

The  next  step  was  to  determine  the  effectiveness  of  the  particle  filter  for  a  moving 
contact.  For  this,  the  ASV  and  the  adversary  drove  past  each  other,  maintaining  their 
respective  courses  and  speeds  as  indicated  by  Figure  5-3  through  Figure  5-7.  The  red 
vessel  is  the  contact  of  interest,  the  yellow  vessel  is  ownship,  and  the  green  balls 
represent  the  generated  particles.  Initially,  the  particles  are  formed  radially  from 
the  first  range  measurement;  as  time  progresses,  the  particles  coalesce  into  larger 
groupings.  At  t=5  (Figure  5-7)  the  particles  have  converged  on  the  contact. 
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pMarineViewer 


Figure  5-3:  Moving  Target  at  t=l:  Once  ownship  received  range  information, 
initialization  of  particles  occur  along  that  radius. 


pMarineViewer 


Figure  5-4:  Moving  Target  at  t—2:  Particle  separation  begins  as  a  result  of 
each  particle  being  initialized  with  different  courses  and  different  speeds. 
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Figure  5-5:  Moving  Target  at  t=3:  Particles  begin  to  form  cluster  with  sim¬ 
ilar  relative  motion  as  a  result  of  the  resampling  subroutine  transforming  the 
particle  distribution  by  drawing  with  replacement  from  the  previous  distribu¬ 
tion. 
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Figure  5-6:  Moving  Target  at  t=4:  Clusters  continue  to  consolidate  after 
additional  resampling  events. 
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Figure  5-7:  Moving  Target  at  t=5:  The  cluster  of  particles  eventually  con¬ 
solidate  to  the  contact’s  location. 


The  Gaussian  Variance  Setting 


Variance 

Average  Solution  Range  Error  (m) 

Highest  Weighted  Particle 

Solution  Range  Error  (m) 

5 

48.11 

41.36 

15 

32.25 

36.07 

25 

40.64 

42.23 

50 

44.54 

42.12 

Table  5.2:  Gaussian  Variance  Error 


Recall  from  Section  2.5  that  the  weights  were  determined  from  the  probability  dis¬ 
tribution  of  the  range  difference  between  the  predicted  range  and  the  sensed  range. 
This  section  shows  the  results  of  a  series  of  trials  used  to  find  a  reliable  setting  for 
the  Gaussian  variance.  If  the  Gaussian  variance  is  set  too  high,  then  the  cloud  of 
particles  will  be  too  large,  making  it  difficult  to  localize  a  solution.  If  the  Gaussian 
Variance  is  set  too  low  then  correct  particles  could  be  mistakenly  discarded  due  to 
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natural  noise.  Table  5.2  shows  the  results  for  different  variance  settings;  based  on  the 
results,  a  variance  of  15  appears  to  produce  the  smallest  average  solution  range  error 
for  these  simulated  conditions. 


The  Course  /  Speed  Noise  Setting 

One  way  to  promote  particle  variation  is  through  the  introduction  of  noise  into  both 
course  and  speed  every  time  a  particle’s  position  is  updated.  One  question  that 
arises  is:  How  much  noise  is  necessary  to  achieve  proper  particle  diversity?  If  too 
little  noise  is  added,  then  particle  variation  is  small  and  an  accurate  solution  is  not 
achieved.  If  too  much  noise  is  added,  an  excess  of  particle  diversity  is  created  and  the 
solution  for  the  contact  of  interest  has  little  fidelity.  Figures  5-8  and  5-9  and  Table 
5.4  show  the  effects  of  various  noise  levels  on  solution  convergence.  Note  that  with  no 
noise  the  particles  have  difficulty  converging  on  a  solution,  and  with  high  noise  the 
particles’  range  error  is  increased.  It  is  also  evident  from  these  figures  that  the  error 
is  significantly  reduced  after  time  step  8;  this  was  due  to  a  maneuver  from  ownship 
that  caused  a  reduction  in  ambiguity. 


Noise  Level 

Range  Error  Before  Maneuver  (m) 

Range  Error  After  Maneuver  (m) 

5° 

53.03 

7.69 

15° 

9.87 

12.02 

50° 

6.83 

5.96 

90° 

8.68 

5.61 

180° 

8.65 

8.56 

Table  5.3:  Average  Course  Noise  Error 
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Noise  Level 

Range  Error  Before  Maneuver  (m) 

Range  Error  After  Maneuver  (m) 

0.5  m/s 

10.47 

5.534 

1  m/s 

41.98 

5.45 

1.5  m/s 

46.87 

7.172 

2  m/s 

49.02 

10.46 

Table  5.4:  Average  Speed  Noise  Error 


Course  Noise 


Time  Steps 

^^Course  Noise  =  5  ^HCourse  Noise  =  15  ^^Course  Noise  =  50 
^•^Course  Noise  =  90  -^Course  Noise  =  lSO^—Maneuver 


Figure  5-8:  Course  Noise:  After  each  resample  step  the  particles  gradually 
consolidate  to  one  particle.  No  significant  improvements  for  localization  were 
seen  after  a  course  noise  of  15°  was  added.  An  important  observation  occurs  at 
time  step  7-8;  here  ownship  maneuvered  and  was  able  to  localize  the  contact. 
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Speed  Noise 


Time  Steps 


■Speed  Noise  =  0.5 


^^Speed  Noise  =  1.0  ^“Speed  Noise  =  1.5 


■^^Speed  Noise  =  2.0  ^“Maneuver 


Figure  5-9:  Speed  Noise:  After  each  resample  step  the  particles  gradually 
consolidate  to  one  particle.  No  significant  improvements  for  localization  were 
seen  after  a  speed  noise  of  0.5 in/s  was  added.  An  important  observation 
occurs  at  time  step  7-8;  here  ownship  maneuvered  and  was  able  to  localize 
the  contact. 


5.2  Simulation  Testing 


This  section  illustrates  a  series  of  scenarios  that  tested  the  adaptive  maneuvering  and 
effectiveness  of  the  particle  filter.  The  location  for  the  simulation  and  real  testing 
was  the  MIT  Sailing  Pavilion  as  seen  in  Figure  5-10. 
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Figure  5-10:  MIT  Sailing  Pavilion:  Image  was  taken  from  Google  Earth. 


5.2.1  Box  Maneuver 

Figure  5-11  is  an  example  of  a  contact  performing  a  basic  box  pattern  while  using 
reserve  particles.  The  average  particle  filter  error  for  the  maneuvering  contact  was 
7.64  meters  for  the  average  hypothesis  and  12.64  meters  for  the  highest  weighted 
hypothesis. 

Figure  5-12  is  an  example  of  a  contact  performing  a  basic  box  pattern  without 
using  reserve  particles.  The  average  particle  filter  error  for  the  maneuvering  contact 
was  9.93  meters  for  the  highest  weighted  hypothesis  and  14.7  meters  for  the  average 
hypothesis.  Notice  there  is  a  slight  benefit  for  using  reserve  particles. 
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Y  Coordinate  (meters) 


Box  Maneuver  with  Gaussian  Error  (Var=1)  with  Reserve  Particles 
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Figure  5-11:  Box  Simulation:  Ownship  was  able  to  track  a  maneuvering 
contact  within  10  meters  of  its  position.  One  of  the  difficulties  with  this 
scenario  was  the  contact  maneuvered  every  100  meters  (60  seconds). 
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Box  Maneuver  with  Gaussian  Error  (Var=1)  without  Reserve  Particles 
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Figure  5-12:  Box  Simulation:  Ownship  was  able  to  track  a  maneuvering 
contact  within  12  meters  of  its  position.  One  of  the  difficulties  with  this 
scenario  was  the  contact  maneuvered  every  100  meters  (60  seconds). 


5.2.2  Bowtie  Maneuver 

Figure  5-13  is  an  example  of  a  contact  performing  a  basic  bowtie  pattern.  The 
average  particle  filter  error  for  the  maneuvering  contact  was  9.5  meters  for  the  highest 
weighted  hypothesis  and  12.2  meters  for  the  average  hypothesis. 

Without  reserve  particles  the  algorithm  could  not  localize  and  track  the  contact. 
The  average  particle  filter  error  for  the  maneuvering  contact  was  17.5  meters  and 
19.6  meters  for  the  highest  weighted  particle.  However,  in  some  trials,  ownship  lost 
complete  track  of  the  contact  especially  as  range  sampling  decreased.  Figure  5-14 
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is  an  great  example  of  the  particle  filter  being  unable  to  match  the  contacts  course 
change. 
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Figure  5-13:  Bowtie  Simulation  with  Reserve  Particles:  Ownship  was  able  to 
track  the  contact  within  12  meters.  The  bowtie  simulation  was  more  difficult 
for  the  algorithm  due  to  the  large  course  changes  that  occurred.  Using  reserve 
particles  provided  a  significant  reduction  of  error. 
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Bowtie  Maneuver  with  Gaussian  Error  (Var=1)  without  Reserve  Particles 


c n 

\ _ 

a; 

a> 

■E 

a; 

oo 


o 

o 

O 

>- 


80 
60 
40 
20 
0 

-20 
-40 
-60 
-80 
-100 


-120 


Target’s  Track 
Average  Hypothesis 
Highest  Weighted  Hypothesis 


X  -  ...\ . 

I  •..•••  NV*5.  */  ’ 

1 ,  •  •  .i 

/  •  . *  ^  i 

•  •  /•  •  • 
•/< •  „  •  •  >  • 

:;w  •••  I 

«•.  •  :•/  *•  •  ->\.  •  I 

‘  •  MVf 


-50 


E1  40 

i _ 

o 

£  20 
CO 

DC  0 


50  100  150 

X  Coordinate  (meters) 


200 


- PF’s  Average  Target  Error  (Gaussian  Var=1) 


/  I 


/  "  ✓  / 
i 


/  V  / 


,/v 


/ 

/  V 


/l 


250 


I  M 

^  1  '  J 


^  /  \  ) 
J  I 


'  J 


20 


40 


60 


80  100 
Time  [Steps] 


120 


140 


160 


Figure  5-14:  Bowtie  Simulation  with  no  Reserve  Particles:  The  approximate 
error  was  above  20  meters  and  eventually  lost  track  of  the  contact.  The 
particle  filter  failed  to  sufficiently  converge  without  the  use  of  reserve  particles. 


Figure  5-15  shows  two  separate  trials  with  different  noise  distributions,  a  uniform, 
and  a  gaussian.  These  distributions  had  similar  results;  however,  one  noticeable 
difference  was  the  uniform  noise  appeared  to  have  higher  deviations  when  the  contact 
maneuvered. 
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Bowtie  with  Gaussian  Error  (Var=1) 


Bowtie  with  Uniform  Error(Var=1) 
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Figure  5-15:  Bowtie  Simulation  with  Reserve  Particles  for  Uniform  and  Gaus¬ 
sian  Distribution:  Tracking  from  a  source  with  a  Gaussian  error  or  a  Uniform 
error  appeared  to  be  similar.  One  noticeable  difference  was  the  uniform  noise 
appeared  to  have  higher  deviations  when  the  contact  maneuvered. 

Ownship  was  able  to  track  the  contact  within  12  meters.  The  bowtie  simula¬ 
tion  was  more  difficult  for  the  algorithm  due  to  the  large  course  changes  that 
occurred.  Using  reserve  particles  provided  a  significant  reduction  of  error. 


5.2.3  Loiter  Maneuver 

Figure  5-16  is  an  example  of  a  contact  performing  a  basic  loitering  pattern.  The 
difficulty  with  this  pattern  is  the  number  of  maneuvers  being  performed  by  the  con¬ 
tact.  It  is  clear  that  the  particle  filter  has  less  difficulty  tracking  this  contact  due  to 
the  small  angle  course  changes.  The  average  particle  filter  error  for  the  maneuvering 
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contact  was  6.34  meters  for  the  average  hypothesis  and  13.2  meters  for  the  highest 
weighted  hypothesis.  Figure  5-17  is  an  example  of  the  same  loiter  pattern  with  a  300 
meter  radius  vice  a  100  meter.  The  accuracy  is  significantly  improved  to  an  average 
particle  filter  error  of  5.18  and  the  highest  weight  hypothesis  error  of  11.5  because  of 
the  longer  time  on  the  track  segment. 
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Figure  5-16:  Loitering  Simulation:  Ownship  was  able  to  continually  track 
the  contact  with  the  particle  filter  with  an  approximate  error  of  12  m.  The 
particle  filter  tracks  with  greater  accuracy  if  the  contact  performs  smaller 
course  changes. 
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Loiter  Maneuver  (Radius=300)  with  Gaussian  Error  (Var=1) 
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Figure  5-17:  Loitering  Simulation:  Ownship  was  able  to  continually  track 
the  contact  with  the  particle  filter  with  an  approximate  error  of  10  m.  The 
particle  filter  tracks  with  greater  accuracy  if  the  contact  performs  smaller 
course  changes  and  maintains  a  longer  time  on  the  track  segment.. 


5.3  Collaborative  ASV  Search 

pParticleFilterAP  has  a  collaborative  ASV  search  aspect  embedded  within.  It  is  based 
on  sharing  range  information  received  by  both  ASV’s  and  using  that  in  their  respective 
particle  filters.  In  the  scenario  listed  (Figure  5-18),  both  patrol  craft  were  conducting 
search  patterns,  once  the  contact  entered  within  a  certain  range,  patrol  craft  2  changed 
from  search  mode  to  pursue.  The  results  of  the  collaborative  particle  filter  appeared 
more  accurate  than  the  single  case.  An  additional  benefit  to  collaborative  search  is  the 


reduction  in  time  for  the  initial  localization;  this  step  was  rapid  because  ambiguous 
solutions  were  removed  as  a  result  of  the  additional  range  source.  The  accuracy 
improvement  was  caused  from  the  information  of  the  additional  range  circle  and  the 
increased  rate  of  range  sampling  caused  by  having  two  sensors.  Lastly,  one  patrol 
craft  can  share  range  information  with  N  other  patrol  craft,  given  the  N  patrol  craft 
the  benefit  of  faster  localization  and  accuracy. 


Collaborating  ASV  Search 
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Figure  5-18:  Collaborative  ASV  Search:  In  this  scenario  two  patrol  craft 
are  initially  conducting  search  patterns  while  sharing  range  information.  As 
the  target  travels  within  500  meters  of  patrol  craft  2,  the  patrol  craft  shifts 
behavior  from  a  search  pattern  to  pursue  the  target. 
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5.4  Charles  River  Testing 


Charle’s  River  Test 


Figure  5-19:  Charles  River  Testing:  The  particle  filter  was  able  to  track 
the  contacts  within  20  m,  note  the  range  measurements  used  were  simulated. 
The  scenario  diverged  slightly  from  the  previous  scenarios  by  having  ownship 
continually  pursue  a  loiter  pattern.  This  was  done  to  avoid  collisions  of  the 
vehicle  and  to  protect  ownship  from  pursing  a  hypothesis  located  on  land. 


Due  to  the  limitations  of  the  test  environment,  the  Charles  River  testing  produced 
mixed  results.  If  the  initial  state  of  the  contact  was  known  (as  mentioned  in  Section 
4.2),  the  particle  filter  with  the  TMA  maneuvers  were  able  to  track  the  contact 
(Figure  5-19).  However,  the  ASV  was  limited  in  its  ability  to  localize  the  contact 
using  only  TMA  maneuvers.  This  was  a  result  of  the  limited  WiFi  coverage  which 
had  a  range  of  100-200  meters.  By  the  time  ownship  completed  the  initialization 


and  TMA  maneuvers,  the  contact  had  already  conducted  a  course  maneuver  to  stay 
within  the  operating  envelope.  Appendix  A  contains  pictures  taken  from  the  testing 
conducted  on  the  Charles  River.  The  average  particle  filter  error  for  the  maneuvering 
contact  was  14.5  meters  for  the  average  weighted  hypothesis  and  19.6  meters  for  the 
highest  weighted  hypothesis. 

5.5  Analysis 

One  method  used  to  check  the  results  of  these  simulations  and  Charles  River  testing 
was  by  comparison  of  the  Cramer- Rao  Lower  Bound  (CLRB)  [41].  A  CRLB  [31]  was 
determine  to  find  the  lower  bound  on  the  variance  of  estimators.  The  bound  is  derived 
from  the  inverse  of  the  Fisher  information  matrix  (FIM).  This  help  determine  the 
expected  variance  based  on  the  sampling  rate  and  variance  of  the  range  measurement. 
The  following  equations  are  a  derivation  of  the  CRLB  from  a  Gaussian  distribution 
with  a  zero  mean  where  L(X,  a2)  is  the  likelihood  function  [5,31,41],  The  Fisher 
Information  Matrix  J  for  a  single  variable  is  the  following: 
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In  this  case  L  is  the  pdf  of  a  Gaussian  function  with  zero  mean  (//). 
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Since  the  CRLB  is  the  inverse  of  the  Fisher  Matrix: 
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var(T)  —< 


1 

J 


so 


var(T ) 


4(<r2)2 
3  n 


(5.7) 


(5.8) 


(5.9) 


(5.10) 


(5.11) 


The  following  figures  were  constructed  to  give  guidance  on  the  variance  of  estimators 
for  the  sailing  pavilion  based  on  the  kingfisher  ASV.  The  maximum  time  steps  used 
was  50,  this  was  based  on  a  max  rate  of  2  m/s  for  the  Kingfisher  with  a  max  WiFi 
range  of  100  m.  The  results  presented  in  this  chapter  approached  the  CRLB  estimate 
with  variances  of  10  to  15  m. 
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Cramer  Rao  Lower  Bound 


Var  =  5  m 


■Sample  Rate  =  3  s 
■Sample  Rate  =  5  s 
■Sample  Rate=  10  s 
■Sample  Rate  =  15  s 


Figure  5-20:  CRLB  with  Variance  of  5  meters 
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Cramer  Rao  Lower  Bound 


Var  =  10  m 
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■Sample  Rate  =  3  s 
■Sample  Rate  =  5  s 
■Sample  Rate=  10  s 
■Sample  Rate  =  15  s 


Figure  5-21:  CRLB  with  Variance  of  10  meters 
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Cramer  Rao  Lower  Bound 


Var=  15  m 


■Sample  Rate  =  3  s 
■Sample  Rate  =  5  s 
■Sample  Rate=  10  s 
■Sample  Rate  =  15  s 


Figure  5-22:  CRLB  with  Variance  of  15  meters 
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Figure  5-23:  CRLB  with  Variance  of  25  meters 
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Chapter  6 


Conclusions 


This  thesis  was  able  to  show  that  a  particle  filter  coupled  with  TMA  based  maneuvers 
could  use  range-only  data  to  localize  and  track  a  single  contact  through  collaborat¬ 
ing  with  multiple  vehicles  or  conducting  adaptive  maneuvering  that  removed  particle 
ambiguity.  While  tracking  stationary  and  moving  contacts  appeared  to  be  reasonable 
for  the  particle  filter,  tracking  maneuvering  contacts  proved  to  be  more  difficult  espe¬ 
cially  as  the  angle  and  frequency  of  the  maneuvers  increased.  Reserve  particles  were 
used  to  aid  in  the  tracking  of  maneuvering  contacts  and  they  proved  to  be  a  viable 
solution.  The  particle  filter  was  able  to  track  a  single  moving  contact  within  5  meters 
of  accuracy  and  a  maneuvering  contact  withinl5  meters. 

It  is  important  to  recall  the  limits  of  this  method  as  noted  from  the  lack  of  ob¬ 
servability  in  the  state  equation  as  discussed  in  Chapter  4  and  the  CRLB  discussed 
in  Chapter  5. 

6.1  Future  Work 

There  are  many  areas  in  which  pParticlcFilterAP  could  be  improved  and  researched. 

1.  The  first  involves  the  pdf  that  was  used  to  determine  the  weights.  This  could  be 
investigated  and  optimized.  One  such  way  to  improve  it  would  be  to  incorporate 
local  land  and  interference  information  and  discard  those  possibilities  from  the 
probability  distribution  those  courses  and  positions  that  are  not  feasible. 
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2.  Deeper  exploration  of  the  particle  filter  parameters  of  noise,  Gaussian  variance, 
and  number  of  particles.  Some  of  these  factors  were  investigated,  it  was  done  so 
empirically.  Finding  an  understanding  of  why  and  how  to  set  the  values  would 
be  useful. 

3.  The  particle  filter  that  was  used  in  this  thesis  was  calibrated  from  the  previously 
mentioned  parameters.  Finding  an  autonomous  way  for  calibrating  would  be 
ideal  for  adaptability  to  numerous  environments. 

4.  The  use  of  different  vehicles,  such  as  the  REMUS  AUV  [38],  could  be  used  to 
conduct  underwater  experiments  and  verifications. 

5.  ft  seems  necessary  to  perform  additional  testing  with  multiple  vehicles,  as  to, 
incorporate  the  capability  for  the  vehicles  to  maneuver  to  optimize  the  perfor¬ 
mance  of  the  tracker.  In  onr  results  in  the  previous  chapters,  the  vehicles  shared 
raw  range  data  to  obtain  a  better  particle  filter  solution.  A  desirable  next  step 
is  to  compute  cooperative  maneuvers  for  the  vehicles  in  order  to  minimize  the 
uncertainty  in  the  target  location,  as  discussed  in  [3,43]. 

6.  Further  research  could  be  to  build  and  compare  more  filters  such  as  a  Maximum 
Likelihood  Estimator  (MLE)  or  an  Extended  Kalman  Filter  (EKF). 

7.  Implement  a  method  to  track  multiple  contacts;  one  of  the  assumptions  that 
was  made  early  on  was  that  there  was  only  one  target.  [13] 

8.  There  is  a  need  to  expand  the  WiFi  Coverage  of  the  Charles  River  to  increase 
the  range  of  the  ASVs  and  the  physical  domain  for  the  testing. 

9.  More  investigation  into  the  state  equations  used  for  observability  is  needed  and 
the  development  of  additional  behaviors  to  maneuver  to  overcome  ambiguity 
would  be  useful. 

10.  It  may  prove  fruitful  to  develop  an  interacting  multiple  modeling  filter  approach 
for  tracking,  and  to  integrated  it  with  the  IvPHelm. 
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11.  Finally,  even  though  the  project  focused  on  processing  sonar  information  for 
submerged  contacts,  the  application  developed  should  be  capable  of  handling 
radar  range  measurements.  It  would  be  interesting  to  adapt  the  MOOS-IvP 
modules  developed  in  this  thesis  for  radar  tracking  of  surface  contacts. 
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Appendix  A 


Charles  River  Testing  Pictures 


Figure  A-l:  Kingfisher  at  sea  testing 
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Figure  A-2:  Kingfisher  at  sea  2 
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Figure  A-4:  Kingfisher  internal  1 


Figure  A-5:  Kingfisher  internal  2 
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Figure  A-6:  Up  Close  Tracking:  In  this  picture  ownship  collided  with  the 
contact.  This  is  an  example  of  how  well  the  tracking  algorithm  works;  however, 
a  safety  method  to  prevent  collisions  needs  to  be  addressed. 
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